#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "richalgo.h"
#include "CGSolver.h"
#include "FDmatrix.h"
#include "clapack.h"

int i,j;
int richardsalgo(double H0[], int solvecase, int iz, double dz,double dt,double material[][3],
                  double k0[],double phi[],double rhomatrix[][3],int iter_t){


        integer iter_z = iz;
        double H[iter_z];
        double Hold[iter_z];
        double H1[iter_z];
        int iter_picar = 1000;
        double value_norm;
        double tol = pow(10,-8);
        integer lda = 4 ;
        double AB[lda*iter_z];
        double b[iter_z];
        integer ipiv[iter_z];
        integer info;
        integer incx = 1;
        integer incy = 1;
        integer nhrs = 1;
        integer kl = 1;
        integer ku = 1;
        int press_iter = 0;




        dcopy_(&iter_z,H0,&incx,H,&incy);
        for(i = 0;i<iter_t;i++){
            if (i >0 ){
            dcopy_(&iter_z,H,&incx,H0,&incy);
            }

            for(j = 0;j<iter_picar;j++){

                    dcopy_(&iter_z,H,&incx,Hold,&incy);

                    createFDmatrix(iter_z,dt,dz,material,phi,k0,H,H0,rhomatrix,AB,b,lda);

                    switch (solvecase){
                    case 1:
                        dgbsv_(&iter_z,&kl,&ku,&nhrs,AB,&lda,ipiv,b,&iter_z,&info);
                        break;
                    case 2:
                        cgmethod(AB,b,H,iter_z,lda);
                        dcopy_(&iter_z,H,&incx,b,&incy);
                        break;
                    default:
                        break;
                    }




                    dcopy_(&iter_z,b,&incx,H1,&incy);
                    double alpha = -1;
                    daxpy_(&iter_z,&alpha,Hold,&incx,b,&incy);
                    value_norm = dnrm2_(&iter_z,b,&incx);

                    if (value_norm < tol){
                        dcopy_(&iter_z,H1,&incx,H,&incy);
                        press_iter = press_iter+(j+1);
                        break;
                    }else{
                         dcopy_(&iter_z,H1,&incx,H,&incy);

                    }

            }

     }

        dcopy_(&iter_z,H1,&incx,H0,&incy);
        return press_iter;

}
